/*
 * File - ros_depth_publisher.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

/**
 * \cond INTERNAL
 */


#include "config.h"

#include <nodelet/nodelet.h>
#include <ros/rate.h>

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/distortion_models.h>

#include <opencv2/opencv.hpp>

#include <cv_bridge/cv_bridge.h>
/**
 * \endcond
 */

#include "ros_depth_publisher.h"
#include "inudev_ros_nodelet.h"

#include "temporal_filter.h" 
#include "space_filter.h"

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/distortion_models.h>

#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/ply_io.h>
/**
 * \file ros_depth_publisher.cpp
 *
 * \brief CRosDepthPublisher
 *
 * This publisher will publish the following topics:
 *
 * - /sensor_msgs/Image/Depth/depth
 */

/**
 * \cond INTERNAL
 */

static const char rcsid[]="$Id$";

/**
 * \endcond
 */

namespace __INUROS__NAMESPACE__
{

    Inuchip::Space_Filter sf;
    Inuchip::TemporalFast tf2;

    CRosDepthPublisher::CRosDepthPublisher(ros::NodeHandle& _node,  std::shared_ptr<image_transport::ImageTransport> _image_transport, CRosSensor* _rosSensor, std::string iTopicName)
        : CRosVideoPublishersBase(_node, _image_transport, "depth", _rosSensor)
        , publisher(imageTransport->advertise(iTopicName, 1))
        #if 1
        ,pcl_pub(node.advertise<sensor_msgs::PointCloud2>("depth2pc", 1))
        #else
        // , pcl_pub(node.advertise<sensor_msgs::PointCloud2>("depth2pc/"+_rosSensor->getSerialNumber(), 1))
        #endif
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
    }


    const unsigned char BLut[256] =
    {

        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        4,   8,  12,  16,  20,  24,  28,  32,  36,  40,  44,  48,  52,  56,  60,  64,
        68,  72,  76,  80,  84,  88,  92,  96, 100, 104, 108, 112, 116, 120, 124, 128,
        132, 136, 140, 144, 148, 152, 156, 160, 164, 168, 172, 176 ,180, 184, 188, 192,
        196, 200, 204, 208, 212, 216, 220, 224, 228, 232, 236, 240, 244, 248, 252, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        252, 248, 244, 240, 236, 232, 228, 224, 220, 216, 212, 208, 204, 200, 196, 192,
        188, 184, 180, 176, 172, 168, 164, 160, 156, 152, 148, 144, 140, 136, 132, 128,
    };
    const unsigned char GLut[256] =
    {
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        4,   8,  12,  16,  20,  24,  28,  32,  36,  40,  44,  48,  52,  56,  60,  64,
        68,  72,  76,  80,  84,  88,  92,  96, 100, 104, 108, 112, 116, 120, 124, 128,
        132, 136, 140, 144, 148, 152, 156, 160, 164, 168, 172, 176, 180, 184, 188, 192,
        196, 200, 204, 208, 212, 216, 220, 224, 228, 232, 236, 240, 244, 248, 252, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        252, 248, 244, 240, 236, 232, 228, 224, 220, 216, 212, 208, 204, 200, 196, 192,
        188, 184, 180, 176, 172, 168, 164, 160, 156, 152, 148, 144, 140, 136, 132, 128,
        124, 120, 116, 112, 108, 104, 100,  96,  92,  88,  84,  80,  76,  72,  68,  64,
        60,  56,  52,  48,  44,  40,  36,  32,  28,  24,  20,  16,  12,   8,   4,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
    };
    const unsigned char RLut[256] =
    {
        0, 136, 140, 144, 148, 152, 156, 160, 164, 168, 172, 176, 180, 184, 188, 192,
        196, 200, 204, 208, 212, 216, 220, 224, 228, 232, 236, 240, 244, 248, 252, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
        252, 248, 244, 240, 236, 232, 228, 224, 220, 216, 212, 208, 204, 200, 196, 192,
        188, 184, 180, 176, 172, 168, 164, 160, 156, 152, 148, 144, 140, 136, 132, 128,
        124, 120, 116, 112, 108, 104, 100,  96,  92,  88,  84,  80,  76,  72,  68,  64,
        60,  56,  52,  48,  44,  40,  36,  32,  28,  24,  20,  16,  12,   8,   4,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
        0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
    };
    cv::Mat Short2RGB(const cv::Mat& imIn)
    {
        cv::Mat imOut(imIn.size(), CV_8UC3);
        //int maxDepth = *max_element(imIn.begin<uint16_t>(), imIn.end<uint16_t>());
        //int minDepth = *min_element(imIn.begin<uint16_t>(), imIn.end<uint16_t>());
        int maxDepth = 6000;//4000
        int minDepth = 0;
        float nomrmalizeFactor = (maxDepth - minDepth);

        for (int r = 0; r < imIn.rows; r++)
        {
            const ushort* in_ptr = imIn.ptr<ushort>(r);
            cv::Vec3b* out_ptr = imOut.ptr<cv::Vec3b>(r);
            for (int c = 0; c < imIn.cols;c++)
            {
                int noramlizedZ = int(255.0f * (in_ptr[c] - minDepth) / nomrmalizeFactor + 0.5f);
                out_ptr[c][0] = BLut[noramlizedZ];
                out_ptr[c][1] = GLut[noramlizedZ];
                out_ptr[c][2] = RLut[noramlizedZ];

            }
        }

        return imOut;
    }    
    
    void CRosDepthPublisher::LoadOD()
    {
        int iSen = 0;

        mOD.focalDepth[0] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[0];
        mOD.focalDepth[1] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[1];

        mOD.centerDepth[0] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[0];
        mOD.centerDepth[1] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[1];

        x0 =mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[0];
        y0 =mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[1];
        fx =1.0F / mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[0];
        fy =1.0F / mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[1];

        mOD.baselineDepth = mCalibrationData->Baselines[std::pair<int,int>(0,1)];


        //
        iSen = 2;

        // auto calibrationData = std::make_shared<InuDev::CCalibrationData>();
        sensor->getSensor()->GetCalibrationData(*mCalibrationData.get(), 4);
        // mCalibrationData = calibrationData; // shared_ptr assignemt

        if( std::stoi((sensor->calibrationVersion).substr(2,2)) < 19)
        {
            if(mRegistrationChannel != -1)
            {
                x0 =mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.OpticalCenter[0];
                y0 =mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.OpticalCenter[1];
                fx =1.0F / mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.FocalLength[0];
                fy =1.0F / mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.FocalLength[1];
            }

            mOD.focalWeb[0] = mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.FocalLength[0];
            mOD.focalWeb[1] = mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.FocalLength[1];
            mOD.centerWeb[0] = mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.OpticalCenter[0];
            mOD.centerWeb[1] = mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.OpticalCenter[1];

            for (size_t i = 0; i < mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.LensDistortion.size(); i++ )
                mOD.KdWeb[i] = mCalibrationData->Sensors[iSen].RealCamera.Intrinsic.LensDistortion[i];
            

            
            mOD.rotAngle.x = mCalibrationData->Sensors[iSen].RealCamera.Extrinsic.Rotation[0];
            mOD.rotAngle.y = mCalibrationData->Sensors[iSen].RealCamera.Extrinsic.Rotation[1];
            mOD.rotAngle.z = mCalibrationData->Sensors[iSen].RealCamera.Extrinsic.Rotation[2];
            // mOD.RotRightWeb = OptData::buildRotation3D(mOD.rotAngle);

            mOD.WebcamTranslate[0] = mCalibrationData->Sensors[iSen].RealCamera.Extrinsic.Translation[0];
            mOD.WebcamTranslate[1] = mCalibrationData->Sensors[iSen].RealCamera.Extrinsic.Translation[1];
            mOD.WebcamTranslate[2] = mCalibrationData->Sensors[iSen].RealCamera.Extrinsic.Translation[2];
        }else{
            if(mRegistrationChannel != -1)
            {
                x0 =mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[0];
                y0 =mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[1];
                fx =1.0F / mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[0];
                fy =1.0F / mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[1];
            }

            mOD.focalWeb[0] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[0];
            mOD.focalWeb[1] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.FocalLength[1];
            mOD.centerWeb[0] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[0];
            mOD.centerWeb[1] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.OpticalCenter[1];

            for (size_t i = 0; i < mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.LensDistortion.size(); i++ )
                mOD.KdWeb[i] = mCalibrationData->Sensors[iSen].VirtualCamera.Intrinsic.LensDistortion[i];
            

            
            mOD.rotAngle.x = mCalibrationData->Sensors[iSen].VirtualCamera.Extrinsic.Rotation[0];
            mOD.rotAngle.y = mCalibrationData->Sensors[iSen].VirtualCamera.Extrinsic.Rotation[1];
            mOD.rotAngle.z = mCalibrationData->Sensors[iSen].VirtualCamera.Extrinsic.Rotation[2];
            // mOD.RotRightWeb = OptData::buildRotation3D(mOD.rotAngle);

            mOD.WebcamTranslate[0] = mCalibrationData->Sensors[iSen].VirtualCamera.Extrinsic.Translation[0];
            mOD.WebcamTranslate[1] = mCalibrationData->Sensors[iSen].VirtualCamera.Extrinsic.Translation[1];
            mOD.WebcamTranslate[2] = mCalibrationData->Sensors[iSen].VirtualCamera.Extrinsic.Translation[2];
        }
        // mOD.uvWeb[0] = mOD.uvWeb[1] = 0;
        mOD.Init();

    }    

    void CRosDepthPublisher::pub_pcl(const cv::Mat &dep, uint64_t curTs,int fidx,bool bOrder,int rinter,int cinter)
    {
        sensor_msgs::PointCloud2Ptr cloudMsg(new sensor_msgs::PointCloud2);

        cv::Mat imgWeb;
        
        float fScale = 1.0F;
        if (CInuDevRosNodelet::config.PC_scale > 1)
            fScale = 1.0F / CInuDevRosNodelet::config.PC_scale;
        uint64_t webTs = sensor->GetWebImage(imgWeb,curTs);
        int rtotal = 0;
        int ctotal = 0;
        // if (imgWeb.empty())
        if (1)
        {
#ifdef SPEED_OPT
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>(dep.cols,dep.rows));
	        int gidx = 0;
            //cloud->points.reserve(dep.rows*dep.cols);
#else
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
#endif
            for (int y = 0; y < dep.rows; y++)
            {
                rtotal++;
                for (int x = 0; x < dep.cols; x++)
                {
                    if (y==0) ctotal++;

                    int z = dep.at<ushort>(y,x);
                    if (!(CInuDevRosNodelet::config.PCL_virtual_wall)){
                        if (z == 0 &&!bOrder) continue;
                    }
                    else{
                        if(z == 0 || z>CInuDevRosNodelet::config.Virtual_wall)
                        {
                            z = CInuDevRosNodelet::config.Virtual_wall;
                        }
                    }
                    int xw = z * (x*cinter - x0) *fx;
                    int yw = z * (y*rinter - y0) *fy;
#ifdef SPEED_OPT
         
                    cloud->points[gidx].x = xw*fScale;
                    cloud->points[gidx].y = yw*fScale;
                    cloud->points[gidx++].z = z*fScale;
#else
                    cloud->points.emplace_back(pcl::PointXYZ(xw*fScale,yw*fScale,z*fScale));
#endif
                }
            }
#ifdef SPEED_OPT
            cloud->points.erase(cloud->points.begin() + gidx, cloud->points.end());
            if (bOrder)
            {
                cloud->width = ctotal;
                cloud->height =rtotal;
            }
            else
            {
                cloud->width = gidx;
                cloud->height =1 ;
            }
#endif
            pcl::toROSMsg(*cloud, *cloudMsg); 
        }
        else
        {
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
#ifdef SPEED_OPT
            cloud->points.reserve(dep.rows*dep.cols);
#endif 

            for (int row = 0; row < dep.rows; row++)
            {
                rtotal++;
                for (int col = 0; col < dep.cols; col++)
                {
                    if (row==0) ctotal++;
                    float z = dep.at<ushort>(row,col);
                    float x3d = 0, y3d = 0;
                    bool bzero = false;
                    int X = col,Y=row;

                    if (!(CInuDevRosNodelet::config.PCL_virtual_wall)){

                        if (z == 0) 
                        {
                            if (!bOrder)
                                continue;
                            else
                                bzero = true;
                        }
                    }
                    else{
                        if(z == 0 || z>CInuDevRosNodelet::config.Virtual_wall)
                        {
                            z = CInuDevRosNodelet::config.Virtual_wall;
                        }
                    }
                    cv::Vec3b v3(255,255,255);
                    if (!bzero)
                    {
                        // if(mOutputFormat == 4)
                        {
                            x3d = z*((float)col - x0)*fx;
                            y3d = z*((float)row - y0)*fy;
                        }
                        if(mRegistrationChannel == -1)
                        {
                            // cv::Point2f uv = OptData::Depth2Web(col*cinter, row*rinter, z, mOD, x3d,y3d);
                            cv::Point3f uv = Inuchip::Depth2Web_Pt(col*cinter, row*rinter, z, mOD, x3d, y3d);

                            X = static_cast<int>(uv.x); 
                            Y = static_cast<int>(uv.y); 
                        }

                        if ((X >= 0 && X < imgWeb.cols) && (Y >= 0 && Y < imgWeb.rows))
                        {
                            if (imgWeb.type() == CV_8UC4)
                            {
                                cv::Vec4b v4 = imgWeb.at<cv::Vec4b>(Y, X);
                                v3[0] = v4[0];v3[1] = v4[1];v3[2] = v4[2];
                            }
                            else
                                v3 = imgWeb.at<cv::Vec3b>(Y, X);
                        }
                        else
                        {
                            if (!bOrder)
                                continue;
                            else
                                bzero = true;
                        }
                    }
                    if (bzero)
                    {
                        x3d = 0;
                        y3d = 0;
                        z = 0;
                    }
                    pcl::PointXYZRGB pt(v3[0],v3[1],v3[2]);
                    if (CInuDevRosNodelet::config.PCL_virtual_wall){
                        if (z == CInuDevRosNodelet::config.Virtual_wall) {
                            pt.r = 255;
                            pt.g = 0;
                            pt.b = 0;
                        }
                    }
                    pt.x = x3d*fScale;
                    pt.y = y3d*fScale;
                    pt.z = z*fScale;
                    
                    cloud->points.emplace_back(pt);
                }
                
            }
            pcl::toROSMsg(*cloud, *cloudMsg); 

        }

        ros::param::param<std::string>("Device_ID", mDepthFrameID, "");
        cloudMsg->header.frame_id = "camera_link"+mDepthFrameID; 
        cloudMsg->header.stamp = ros::Time( curTs / 1000000000ll, curTs % 1000000000ll);
        cloudMsg->header.seq = fidx;
        if (bOrder)
        {
            cloudMsg->width = ctotal;
            cloudMsg->height = rtotal;
        }
        pcl_pub.publish(cloudMsg);

    }


    void CRosDepthPublisher::FrameCallback(std::shared_ptr<InuDev::CDepthStream> iStream, std::shared_ptr<const InuDev::CImageFrame> iFrame, InuDev::CInuError iError)
    {
        NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        if (!CheckFrame(iFrame, iError))
        {
            return;
        }

        InuDev::EImageFormat format = (InuDev::EImageFormat)iFrame->Format();

        if (format != InuDev::EImageFormat::eDepth)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Depth Frame's format (" << format << ") is not eDepth");
            return;
        }

        settingConfidence(CInuDevRosNodelet::config.Confidence,false);


        int width = iFrame->Width();
        int height = iFrame->Height();
        int bpp = iFrame->BytesPerPixel();
        const InuDev::byte *data = iFrame->GetData();

        cv::Mat frame(height, width, CV_16UC1, (char *)data);
        frame.at<ushort>(0,0) = 0;   //remove a noise
        int rinter = 1;
        int cinter = 1;
        if (CInuDevRosNodelet::config.Simplify_2)
        {
            rinter = 2;
            cinter = 2;
        }

        if ((rinter > 1 && cinter > 1) && width > 10 && height > 10)
        {
         //   cv::pyrDown(frame,frame, cv::Size(int(width/cinter),int(height/rinter)));
            cv::resize(frame,frame, cv::Size(int(width/cinter),int(height/rinter)),0,0,cv::INTER_NEAREST);
        }

        //first temporal  
        if (CInuDevRosNodelet::config.Temporal_Filter_ROS)
        {
            Inuchip::TemporalFilter tf(CInuDevRosNodelet::config.Temporal_BufferFrames,
                               CInuDevRosNodelet::config.Temporal_ProcessAllPixel,
                               CInuDevRosNodelet::config.Temporal_Threads);
            //TemporalFilter tf;
            frame = tf.process_frame(frame);
        }

        if (CInuDevRosNodelet::config.Passthrough_Filter_ROS)
        {
            frame = sf.process_passthrough(frame,CInuDevRosNodelet::config.PT_zmin,CInuDevRosNodelet::config.PT_zmax
                                                ,CInuDevRosNodelet::config.PT_xmin,CInuDevRosNodelet::config.PT_xmax 
                                                ,CInuDevRosNodelet::config.PT_ymin,CInuDevRosNodelet::config.PT_ymax); 
        }

        if (CInuDevRosNodelet::config.Outlier_Remove_ROS)
        {
            frame = sf.process_outlier_remove(frame,CInuDevRosNodelet::config.OR_percent * 0.001f
                                                   ,CInuDevRosNodelet::config.OR_mindist
                                                   ,CInuDevRosNodelet::config.OR_maxremove);
        }

        //add HoleFile 
        if (CInuDevRosNodelet::config.Hole_Fill_ROS)
        {
            frame = sf.process_hole_fill(frame,CInuDevRosNodelet::config.HF_maxradius); 
        }
        // edge preserve filter
        if (CInuDevRosNodelet::config.Edge_Preserve_ROS)
        {
            frame = sf.process_edge_preserve(frame,CInuDevRosNodelet::config.EP_alpha * 0.01f
                                                   ,CInuDevRosNodelet::config.EP_delta);
        }

        if (CInuDevRosNodelet::config.Temporal_Filter2_ROS)
        {
            frame = tf2.process_frame(frame,CInuDevRosNodelet::config.Temporal_Filter2_alpha * 0.01f
                                                   ,CInuDevRosNodelet::config.Temporal_Filter2_delta);
        }

        pub_pcl(frame, iFrame->Timestamp,iFrame->FrameIndex,CInuDevRosNodelet::config.PC_order,rinter,cinter);

        if (CInuDevRosNodelet::config.Show_Color) 
            frame = Short2RGB(frame);

        std_msgs::Header frameHeader;
        frameHeader.stamp = ros::Time( iFrame->Timestamp / 1000000000ll, iFrame->Timestamp % 1000000000ll);
        frameHeader.seq = iFrame->FrameIndex;

        ros::param::param<std::string>("Device_ID", mDepthFrameID, "");
        frameHeader.frame_id = "camera_link"+mDepthFrameID;

        if (CInuDevRosNodelet::config.Show_Color == false) {
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(frameHeader, "mono16", frame).toImageMsg();
            publisher.publish(msg);
        }
        else {
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(frameHeader, "bgr8", frame).toImageMsg();
            publisher.publish(msg);
        }


        if (CInuDevRosNodelet::config.depth_verbose)
        {
            NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Depth Frame: width=" << width << ", height=" << height << ", format=" << format << ", bpp=" << bpp);
            static auto t1 = std::chrono::steady_clock::now();
            static int sidx = 0;
            sidx++;
            if (sidx >= 10)
            {
                double dr_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - t1).count();

                t1 = std::chrono::steady_clock::now();

                NODELET_WARN_STREAM("Depth Time: " << dr_ms << ", FPS: " << int(1000 / (dr_ms/10)));
                sidx = 0;
            }

            // cv::imshow(CV_WINDOW_NAME, frame); // Show our image inside it.
            // cv::waitKey(1);
        }
    }

    InuDev::CInuError CRosDepthPublisher::RegisterCallback()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CDepthStream::CallbackFunction callback = std::bind(&CRosDepthPublisher::FrameCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

        return std::static_pointer_cast<InuDev::CDepthStream>(stream)->Register(callback);
    }

    InuDev::CInuError CRosDepthPublisher::UnregisterCallback()
    {
        return std::static_pointer_cast<InuDev::CDepthStream>(stream)->Register(nullptr);
    }

    int CRosDepthPublisher::GetNumSubscribers()
    {
        publisherPrev = publisherCurr;
        publisherCurr = publisher.getNumSubscribers() + pcl_pub.getNumSubscribers();

        return publisherCurr;
    }

    InuDev::CInuError CRosDepthPublisher::InitStream()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CInuError err(0);
        
        ros::param::param<bool>("Hole_Fill_SDK", mHoleFilterSDK, false);
        if(CInuDevRosNodelet::config.Hole_Fill_SDK != false)
        {
            mHoleFilterSDK = true;
        }
        
        ros::param::param<bool>("Outlier_Remove_SDK", mOutlierRemoveSDK, false);
        if(CInuDevRosNodelet::config.Outlier_Remove_SDK != false)
        {
            mOutlierRemoveSDK = true;
        }
        
        ros::param::param<bool>("Temporal_Filter_SDK", mTemporalFilterSDK, false);
        if(CInuDevRosNodelet::config.Temporal_Filter_SDK != false)
        {
            mTemporalFilterSDK = true;
        }
        
        ros::param::param<bool>("Passthrough_Filter_SDK", mPassthroughFilterSDK, false);
        if(CInuDevRosNodelet::config.Passthrough_Filter_SDK != false)
        {
            mPassthroughFilterSDK = true;
        }
        
        int tmpPostProcessing = 0;//mPostProcessing;
        #ifndef _GE428_
        if(mRegistrationChannel != -1)
        {
	        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate registeration! ");
        	if(!(tmpPostProcessing & InuDev::CDepthStream::EPostProcessing::eRegistration))
        	{
        		tmpPostProcessing += InuDev::CDepthStream::EPostProcessing::eRegistration;
        	}
        }
        #endif

        if(mHoleFilterSDK)
        {
	        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate static temporal from SDK with num threads: " << CInuDevRosNodelet::config.Temporal_Threads);
        	if(!(tmpPostProcessing & InuDev::CDepthStream::EPostProcessing::eHoleFill))
        	{
        		tmpPostProcessing += InuDev::CDepthStream::EPostProcessing::eHoleFill;
        	}
        } 
        
        if(mOutlierRemoveSDK)
        {
	        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate outlier removal from SDK with precent: " << CInuDevRosNodelet::config.OR_percent);
        	if(!(tmpPostProcessing & InuDev::CDepthStream::EPostProcessing::eOutlierRemove))
        	{
        		tmpPostProcessing += InuDev::CDepthStream::EPostProcessing::eOutlierRemove;
        	}
        } 
        
        if(mTemporalFilterSDK)
        {
	        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate temporal filter from SDK with num buffers: " << CInuDevRosNodelet::config.Temporal_BufferFrames);
        	if(!(tmpPostProcessing & InuDev::CDepthStream::EPostProcessing::eStaticTemporal))
        	{
        		tmpPostProcessing += InuDev::CDepthStream::EPostProcessing::eStaticTemporal;
        	}
        } 

        if (mParamsChannelID == -1  && mOutputFormat == 0 &&  tmpPostProcessing ==0)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: Depth Init with default setting!");
            InuDev::CInuError err = std::static_pointer_cast<InuDev::CDepthStream>(stream)->Init();
        }
        else
        {
            #ifdef _GE428_
                err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->Init((InuDev::CDepthStream::EOutputFormat)mOutputFormat, (unsigned int)tmpPostProcessing);
            #else
            if (mRegistrationChannel != -1)
            {
                err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->Init((InuDev::CDepthStream::EOutputFormat)mOutputFormat, (unsigned int)tmpPostProcessing,(unsigned int)mRegistrationChannel);
            }
            else
            {
                err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->Init((InuDev::CDepthStream::EOutputFormat)mOutputFormat, (unsigned int)tmpPostProcessing);
            }
            #endif

            if(mHoleFilterSDK)
            {
            	NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate hole fill from SDK with max radius: " << CInuDevRosNodelet::config.HF_maxradius);
            	InuDev::CHoleFillFilterParams params;
            	params.MaxRadius = CInuDevRosNodelet::config.HF_maxradius;
            	err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->SetHoleFillParams(params);
            }
            
            if(mOutlierRemoveSDK)
            {
            	NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate outlier remove from SDK with precent: " << CInuDevRosNodelet::config.OR_percent);
            	InuDev::COutlierRemoveParams params;
            	params.MaxPercent = CInuDevRosNodelet::config.OR_percent * 0.01f;
            	params.MinDist = CInuDevRosNodelet::config.OR_mindist;
            	err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->SetOutlierRemoveParams(params);
            }
            
            if(mTemporalFilterSDK)
            {
            	NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate static temporal filter from SDK with num threads: " << CInuDevRosNodelet::config.Temporal_Threads);
            	InuDev::CStaticTemporalFilterParams params;
            	params.FilterLength = CInuDevRosNodelet::config.Temporal_BufferFrames;
            	params.BAll = CInuDevRosNodelet::config.Temporal_ProcessAllPixel;
            	params.ThreadNum = CInuDevRosNodelet::config.Temporal_Threads;
            	err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->SetStaticTemporalFilterParams(params);
            }
        }
        
        if(mPassthroughFilterSDK)
        {
               NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate Passthrough filter from SDK with x max: " << CInuDevRosNodelet::config.PT_zmax);
               InuDev::CDisparityParams params;
               err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->GetDisparityParams(params);
               if (err != InuDev::EErrorCode::eOK)
               {
                   NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " GetDisparityParams return an error: "<< " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
                   return err;
               }
               
               if (CInuDevRosNodelet::config.PT_zmax != -1)
               {
                   params.MaxDistance = CInuDevRosNodelet::config.PT_zmax;
               }

               if (CInuDevRosNodelet::config.PT_zmin != -1)
               {
                   params.MinDistance = CInuDevRosNodelet::config.PT_zmin;
               }


               if (CInuDevRosNodelet::config.PT_zmin != -1 || CInuDevRosNodelet::config.PT_zmax != -1)
               {
                   NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " oded: activate Passthrough filter from SDK with x max: " << CInuDevRosNodelet::config.PT_zmax);
                   err = std::static_pointer_cast<InuDev::CDepthStreamExt>(stream)->SetDisparityParams(params);
               }
        }
          
        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        return InuDev::EErrorCode::eOK;
    }
    
    InuDev::CInuError CRosDepthPublisher::StartStream()
    {
        // Load default params from server
        ros::param::param<int>("DepthChannel", mParamsChannelID, -1);
        ros::param::param<int>("DepthOutputFormat", mOutputFormat, -1);
        ros::param::param<int>("DepthRegistarionChannel", mRegistrationChannel, -1);
        ros::param::param<int>("DepthConfidence", mDepthconfidence, -1);

        // check for user dynamic changes
        if(CInuDevRosNodelet::config.Depth_channel_ID != -1)
        {
            mParamsChannelID = CInuDevRosNodelet::config.Depth_channel_ID;
        }

        if(CInuDevRosNodelet::config.Depth_output_format != -1)
        {
            mOutputFormat = CInuDevRosNodelet::config.Depth_output_format;
        }

        if(CInuDevRosNodelet::config.Depth_registration_channel != -1)
        {
            mRegistrationChannel = CInuDevRosNodelet::config.Depth_registration_channel;
        }

        stream = std::static_pointer_cast<InuDev::CBaseStream>(sensor->getSensor()->CreateDepthStreamExt(mParamsChannelID));

        NODELET_ERROR_STREAM_COND(!stream, __INUROS_FUNCTION_NAME__ << ": failed creating " << getName());

        InuDev::EErrorCode err = CRosPublisher::StartStream();

        if (err != InuDev::EErrorCode::eOK)
        {   
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        settingConfidence(mDepthconfidence,true);

        uint32_t channelID;
        err = stream->GetChannel(channelID);
        
        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        mChannelID = int(channelID);

        auto calibrationData = std::make_shared<InuDev::CCalibrationData>();

        err = sensor->getSensor()->GetCalibrationData(*calibrationData.get(), mChannelID);

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Failed to retreive calibration data: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        mCalibrationData = calibrationData; // shared_ptr assignemt
        for(int i = 0; i < 5; i ++)
        {
            NODELET_WARN_STREAM( "Sen " << i <<  ",FocalLength:" << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.FocalLength[0] << "," << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.FocalLength[1] );
            NODELET_WARN_STREAM("OpticalCenter:" << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.OpticalCenter[0] << "," << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.OpticalCenter[1] );
        }

        LoadOD();

        mSensors = sensor->GetSensors(mChannelID);
        
        return InuDev::EErrorCode::eOK;
    }

    int CRosDepthPublisher::settingConfidence(int newconfidence, bool direct)
    {
        if(newconfidence != mDepthconfidence || direct)
        {
            mDepthconfidence = newconfidence;
            sensor->getSensor()->SetSensorRegister(0x08010ae0, (mDepthconfidence*2+1)<<24, 1, InuDev::eLeft);
            sensor->getSensor()->SetSensorRegister(0x080109e0, (mDepthconfidence*2+1)<<24, 1, InuDev::eRight);
            // NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " mDepthconfidence = " << mDepthconfidence);
        }
        return 0;
    }
}
